import socket
import struct
import sys
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray
import rclpy.qos
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Pose, Twist
from geometry_msgs.msg import PoseStamped, TransformStamped
from interfaces_consensus.msg import CommandMsg
from builtin_interfaces.msg import Time
from geometry_msgs.msg import Pose
from geometry_msgs.msg import Point, Quaternion
from rclpy.parameter import Parameter
import time
import threading

class GoalNode(Node):
    def __init__(self):
    # 定义本地接收地址和端口

        super().__init__('goal_node')

        self.uav_id=int(sys.argv[1])
        self.virtual_uav_publishers = {}
        self.colony_list = [0]  #默认值
        self.cluster_count = -1 #记录当前集群消息数值
        self.group_count = -1 #记录当前分群消息数值
        self.instruction_count = -1 #记录当前导调消息数值
        # self.uav_id=1

        self.declare_parameter('calculated_obstacles')
        
        # # UDP点对点发送
        # self.server_address = (str(sys.argv[2]), 12345)
        # # self.server_address = (''127.0.0.1'', 12345)
        # self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) #创建套接字
        # self.sock.bind(self.server_address) # 绑定套接字到指定地址和端口

        # UDP多播接收
        MCAST_GRP = '224.1.11.111'
        MCAST_PORT = 5007
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)
        self.sock.bind(('', MCAST_PORT))
        mreq = struct.pack("=4sl", socket.inet_aton(MCAST_GRP), socket.INADDR_ANY)
        # self.sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq)
        # 尝试加入多播组，直到成功
        while True:
            try:
                self.sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq)
                time.sleep(1)
                print(f"成功加入多播组 {MCAST_GRP} 并绑定端口 {MCAST_PORT}")
                break  # 成功后跳出循环
            except OSError as e:
                print(f"错误: {e}. 尝试重新连接...")
                time.sleep(1)  # 等待 1 秒后重试
        # self.sock.setblocking(False)  #设置为非阻塞模式
        # self.sock.settimeout(1.0)  #设置超时

        

        # 生成发布命令话题
        qos_profile_1 = rclpy.qos.QoSProfile(
        reliability=rclpy.qos.ReliabilityPolicy.RELIABLE, 
        history=rclpy.qos.HistoryPolicy.KEEP_LAST,              
        depth=10
        )
        cmd_topic = "/drone{}/consensus_APF/command".format(self.uav_id)
        self.publisher_cmd = self.create_publisher(CommandMsg, cmd_topic, qos_profile_1)

        #定义定时器运行主体程序
        # self.create_timer(0.001,self.run)

        # #启动接收数据的线程
        # self.thread = threading.Thread(target=self.run)
        # self.thread.daemon = True
        # self.thread.start()

        

        print("Waiting to receive message...")
        self.run()

    def virtual_uav_pub(self, uavId, msg):
        topic_name = f"drone{uavId}/velocity_and_position"
        if topic_name in self.virtual_uav_publishers:
            publisher = self.virtual_uav_publishers[topic_name]
            # print(f"topic: {topic_name} already existed!")
        else:
            publisher = self.create_publisher(Float64MultiArray, topic_name, 10)
            self.virtual_uav_publishers[topic_name] = publisher
            # print(f"topic: {topic_name} publish!")
        publisher.publish(msg)

    def find_element_with_id(self, arr):
        # 遍历二维数组中的每个子数组
        for subarray in arr:
            # 如果子数组中包含 target_id，返回这个子数组
            if self.uav_id in subarray:
                return subarray
        # 如果没有找到，返回 None
        return None

    def run(self):

        # 解包数据
        # 'ii64s' -> 两个整数和一个最多64字符的字符串
        # '????????' -> 8个布尔值
        while rclpy.ok():
            try:
                
                # 接收数据
                data, address = self.sock.recvfrom(4096)
                # print(data)
                # print(len(data))
                # 解包数据，'d' 表示 double（8字节）
                unpacked_data = struct.unpack("2c", data[:2])
                type = unpacked_data[0]
                head = unpacked_data[1]
                # print(f"type={type}--head={head}")

                #0 导调
                #1 集群状态
                #2 分群
                #3 障碍物位姿
                #4 无人机位姿

                #5 分群结果指令
                #6 控制指令包括目标点、队形、除分群外的其他指令
                
                if head == 1:
                    unpacked_data = struct.unpack('=diiiiii', data[16:])
                    time = unpacked_data[0]
                    colonyID = unpacked_data[1]
                    subcolonyID = unpacked_data[2]
                    currentDroneNumber = unpacked_data[3]
                    formationType = unpacked_data[4]
                    taskID = unpacked_data[5]
                    taskType = unpacked_data[6]
                    
                    # 输出接收到的内容
                    print(f"Received data from {address}:\n time={time}, colonyID={colonyID}, subcolonyID={subcolonyID}, "
                        f"currentDroneNumber={currentDroneNumber}, formationType={formationType}, taskID={taskID}, taskType={taskType}")
                    
                elif head == 2:
                    unpacked_data = struct.unpack('iiii32siid', data[16:])
                    colonyID = unpacked_data[0]
                    subcolonyID = unpacked_data[1]
                    formationType = unpacked_data[2]
                    uavIndex = unpacked_data[3]
                    uavName = unpacked_data[4].decode('utf-8')
                    taskID = unpacked_data[5]
                    taskType = unpacked_data[6]
                    time = unpacked_data[7]

                    # 输出接收到的内容
                    print(f"Received data from {address}:\n colonyID={colonyID}, subcolonyID={subcolonyID}, formationType={formationType}, "
                        f"uavIndex={uavIndex}, uavName={uavName}, taskID={taskID}, taskType={taskType}, time={time}")
                    


                elif head == 3:
                    num_pos = len(data[12:]) / 8
                    # print(f"{len(data[12:])}")
                    obs_pos = list(struct.unpack(f"{int(num_pos)}d", data[16:]))
                    # print(type(obs_pos))
                    # obs_pos_test = []
                    obstacle_positions_param = Parameter('calculated_obstacles',  # 重新将参数值设置为指定值
                            Parameter.Type.DOUBLE_ARRAY, obs_pos)
                    self.set_parameters([obstacle_positions_param])

                    # 输出接收到的内容
                    print(f"Received data from {address}:\n obs_pos={obs_pos}")

                elif type == b'\x24' and head == b'\x03':
                    index = 0 #用于记录当前索引值
                    unpacked_data = struct.unpack('=ii', data[2:10])
                    count = unpacked_data[0]
                    clusterIssuedNum = unpacked_data[1] #集群数量
                    if self.cluster_count == count:
                        continue
                    else:
                        self.cluster_count = count
                    index = 10
                    # 输出接收到的内容
                    print(f"Received data from {address}:\n clusterIssuedNum={clusterIssuedNum}")
                    # 解析每个集群的信息
                    for i in range(clusterIssuedNum):
                        unpacked_data = struct.unpack('=iiididd', data[index:index+40])
                        clusterId = unpacked_data[0] #集群编号
                        camp = unpacked_data[1]  #阵营 0：红方 1：蓝方
                        formation = unpacked_data[2]  #队形
                        interval = unpacked_data[3] #队形间距
                        # 输出接收到的内容
                        print(f"--clusterId={clusterId}, camp={camp}, formation={formation}, interval={interval}")
                        #解析当前集群包含的无人机编号
                        uavNums = unpacked_data[4]
                        safe_distance = unpacked_data[5]
                        cluster_velocity = unpacked_data[6]
                        print(f"--uavnum={uavNums}, safe_distance={safe_distance}, cluster_velocity={cluster_velocity}")
                        index = index+40
                        colonyIDs = [0]*uavNums
                        print("-------------")
                        for j in range(uavNums):
                            unpacked_data = struct.unpack('=iiddd', data[index:index+32])
                            uavId = unpacked_data[0] #无人机id
                            uavType = unpacked_data[1] #2：实体uav 0:虚拟uav 
                            lon = unpacked_data[2]  #初始位置
                            lat = unpacked_data[3]
                            alt = unpacked_data[4]
                            index += 32
                            # 输出接收到的内容
                            print(f"uavId:{uavId},uavType:{uavType},position:[{lon},{lat},{alt}]")
                            colonyIDs[j] = uavId
                            #如果是虚拟uav，需要将位置发送到话题
                            if uavType == 0:
                                # self.publisher_ = self.create_publisher(Float64MultiArray, f"drone{uavId}/velocity_and_position", 10)
                                msg = Float64MultiArray()
                                msg.data = [8.8, lon, lat, alt]
                                # self.publisher_.publish(msg)
                                self.virtual_uav_pub(uavId,msg)
                        if self.uav_id in colonyIDs:
                            msg = CommandMsg()
                            msg.stamp = self.get_clock().now().to_msg()
                            msg.frame_id = "cluster"
                            msg.clusterid = clusterId
                            msg.camp = camp
                            msg.formation = formation + 1
                            msg.interval = interval

                            msg.safe_distance = safe_distance
                            msg.cluster_velocity = cluster_velocity

                            # 动态变化的整数数组
                            msg.dynamic_array = colonyIDs
                            self.colony_list = colonyIDs
                            self.publisher_cmd.publish(msg)
                            print (f"uavid:{self.uav_id} belong to cluster:{colonyIDs}")
                        else:
                            print (f"uavid:{self.uav_id} don't belong to cluster:{colonyIDs}")

                            
                        print("-------------")
                            
                elif type == b'\x10' and head == b'\x01':
                    unpacked_data = struct.unpack('=ii', data[2:10])
                    count = unpacked_data[0]
                    uavNums = unpacked_data[1]
                    if self.group_count == count:
                        continue
                    else:
                        self.group_count = count
                    print(f"uavNums:{uavNums}")
                    index = 10 #记录当前索引位置
                    receive_ids = []
                    for i in range(uavNums):
                        unpacked_data = struct.unpack('=iii', data[index:index + 12])
                        clusterId = unpacked_data[0]  #集群编号
                        childClusterId = unpacked_data[1]  #子集群编号
                        uavId = unpacked_data[2] 
                        index += 12
                        unpacked_data = struct.unpack('=ddd', data[index:index + 24])
                        lon = unpacked_data[0]  #目标点
                        lat = unpacked_data[1]
                        alt = unpacked_data[2]
                        receive_ids.append((clusterId, childClusterId, uavId, lon, lat, alt))
                        index += 24
                        print(f"clusterId:{clusterId},childClusterId:{childClusterId},uavId:{uavId},position:[{lon},{lat},{alt}]")
                    
                    # 找出与自己id相同的子集群列表
                    target_clusterId = None
                    target_childClusterId = None

                    for find_clusterId, find_childClusterId, uavId, lon, lat, alt in receive_ids:
                        if uavId == self.uav_id:
                            target_clusterId = find_clusterId
                            target_childClusterId = find_childClusterId
                            break  
                    if target_clusterId is not None and target_childClusterId is not None:
                        colonyIDs = [uavId for (find_clusterId, find_childClusterId, uavId, lon, lat, alt) in receive_ids
                                        if find_clusterId == target_clusterId]
                        subcolonyIDs = [uavId for (find_clusterId, find_childClusterId, uavId, lon, lat, alt) in receive_ids
                                            if find_clusterId == target_clusterId and find_childClusterId == target_childClusterId]
                        self.colony_list = subcolonyIDs
                        match_pos = [(lon,lat,alt) for (find_clusterId, find_childClusterId, uavId, lon, lat, alt) in receive_ids
                                            if find_clusterId == target_clusterId and find_childClusterId == target_childClusterId]
                        #计算平均值
                        if match_pos:
                            avg_lon = sum(lon for lon,_,_ in match_pos)/len(match_pos)
                            avg_lat = sum(lat for _,lat,_ in match_pos)/len(match_pos)
                            avg_alt = sum(alt for _,_,alt in match_pos)/len(match_pos)
                            print(f"avg_lon:{avg_lon},avg_lat:{avg_lat},avg_alt:{avg_alt}")
                        find_index = self.colony_list.index(self.uav_id)
                        print(f"find_index{find_index}")
                        print(f"match_pos{match_pos}")

                        # #取领航者位置
                        # match_pos_at_index = match_pos[0]
                        # # print(f"match_pos_at_index{match_pos_at_index}")
                        # avg_lon = match_pos_at_index[0]
                        # avg_lat = match_pos_at_index[1]
                        # avg_alt = match_pos_at_index[2]

                        #当前无人机任务部署地点
                        find_index = self.colony_list.index(self.uav_id)
                        match_task_pos_at_index = match_pos[find_index]
                        # print(f"match_pos_list{match_pos_at_index}")
                        own_lon = match_task_pos_at_index[0]
                        own_lat = match_task_pos_at_index[1]
                        own_alt = match_task_pos_at_index[2]

                        print(f"uavid:{self.uav_id} find cluster:{colonyIDs}")
                        print(f"uavid:{self.uav_id} find childCluster:{subcolonyIDs}")

                        #发送集群结果
                        msg = CommandMsg()
                        msg.stamp = self.get_clock().now().to_msg()
                        msg.frame_id = "grouping"
                        msg.clusterid = target_clusterId
                        msg.childclusterid = target_childClusterId

                        # 目标点
                        msg.pose.position = Point(x=avg_lon, y=avg_lat, z=avg_alt)
                        msg.pose.orientation = Quaternion(x=own_lon, y=own_lat, z=own_alt, w=1.0)


                        # 动态变化的整数数组
                        msg.dynamic_array = subcolonyIDs
                        self.publisher_cmd.publish(msg)
                    else:
                        print(f"uavid:{self.uav_id} dont find cluster")
                elif type == b'\x20' and head == b'\x02':
                    # print("导调")
                    unpacked_data = struct.unpack('iiii', data[2:18])
                    count = unpacked_data[0]
                    directionType = unpacked_data[1]  #导调命令类型：1-修改队形 2-返航
                    clusterId = unpacked_data[2]  #集群id
                    formationType = unpacked_data[3]  # 队形参数： 0-横 1-竖 2-三角
                    if self.instruction_count == count:
                        continue
                    else:
                        self.instruction_count = count
                    # 输出接收到的内容
                    print(f"directionType:{directionType},ClusterId:{clusterId},formationType:{formationType}")
                    if directionType == 1:
                        msg = CommandMsg()
                        msg.stamp = self.get_clock().now().to_msg()
                        msg.frame_id = "formation"
                        msg.clusterid = clusterId
                        msg.formation = formationType + 1
                        self.publisher_cmd.publish(msg)
                    elif directionType == 2:
                        msg = CommandMsg()
                        msg.stamp = self.get_clock().now().to_msg()
                        msg.frame_id = "return_to_home"
                        msg.clusterid = clusterId
                        self.publisher_cmd.publish(msg)
                    else:
                        print(f"Unknown instruction received")

                elif type == b'\x30' and head == b'\x03':
                    unpacked_data = struct.unpack('i', data[2:6])
                    uavNums = unpacked_data[0]
                    # print(f"uavNums:{uavNums}")
                    index = 6 #记录当前索引位置
                    for i in range(uavNums):                   
                        unpacked_data = struct.unpack('i', data[index:index + 4])
                        uavId = unpacked_data[0] 
                        index += 4
                        unpacked_data = struct.unpack('ddd', data[index:index + 24])
                        lon = unpacked_data[0]  
                        lat = unpacked_data[1]
                        alt = unpacked_data[2]
                        index += 24
                        # print(f"uavId:{uavId},position:[{lon},{lat},{alt}]")
                        if self.uav_id == self.colony_list[0] and (uavId in self.colony_list):
                            msg = Float64MultiArray()
                            msg.data = [8.8, lon, lat, alt]
                            self.virtual_uav_pub(uavId,msg)

                        
                    
                elif head == 4:
                    unpacked_data = struct.unpack('dii32s9d8?', data[16:])
                    time = unpacked_data[0]
                    model = unpacked_data[1]
                    node = unpacked_data[2]
                    camp = unpacked_data[3].decode('utf-8')
                    position = [unpacked_data[4],unpacked_data[5],unpacked_data[6]]
                    velocity = [unpacked_data[7],unpacked_data[8],unpacked_data[9]]
                    attitude = [unpacked_data[10],unpacked_data[11],unpacked_data[12]]
                    detectSwitch = unpacked_data[13]
                    jamSwitch = unpacked_data[14]
                    investigateSwitch = unpacked_data[15]
                    castSwitch = unpacked_data[16]
                    a = unpacked_data[17]
                    b = unpacked_data[18]
                    c = unpacked_data[19]
                    d = unpacked_data[20]

                    # 输出接收到的内容
                    print(f"Received data from {address}:\n time={time}, model={model}, node={node}, camp={camp}, "
                        f"position={position}, velocity={velocity}, attitude={attitude}, detectSwitch={detectSwitch}, jamSwitch={jamSwitch},"
                        f"investigateSwitch={investigateSwitch}, castSwitch={castSwitch}, a={a}, b={b}, c={c}, d={d}")
                elif head == 5:
                    subcolonyIDs = [
                        [0, 0, 0, 0, 0, 0, 0, 0],  # subcolonyID_1
                        [0, 0, 0, 0, 0, 0, 0, 0],  # subcolonyID_2
                        [0, 0, 0, 0, 0, 0, 0, 0],  # subcolonyID_3
                        [0, 0, 0, 0, 0, 0, 0, 0],  # subcolonyID_4
                        [0, 0, 0, 0, 0, 0, 0, 0],  # subcolonyID_5
                        [0, 0, 0, 0, 0, 0, 0, 0],  # subcolonyID_6
                        [0, 0, 0, 0, 0, 0, 0, 0],  # subcolonyID_7
                        [0, 0, 0, 0, 0, 0, 0, 0],  # subcolonyID_8
                    ]
                    unpacked_data = struct.unpack('64i', data[16:])
                    for i in range(8):  # 有8个subcolonyID列表
                        subcolonyIDs[i] = unpacked_data[i * 8 : (i + 1) * 8]  # 每次8个元素

                    # 结果检查
                    for idx, subcolonyID in enumerate(subcolonyIDs):
                        print(f"subcolonyID_{idx + 1}:", subcolonyID)

                    filtered_subcolonyIDs = [list(filter(lambda x: x != 0, row)) for row in subcolonyIDs if any(x != 0 for x in row)]

                    print(filtered_subcolonyIDs)
                    result = self.find_element_with_id(filtered_subcolonyIDs)
                    if result:
                        print(f"Found the subcolony: {result}")
                        msg = PoseStamped()
                        # 设置头部信息
                        msg.header.stamp = self.get_clock().now().to_msg()
                        msg.header.frame_id = "grouping"  # 可以根据需要调整坐标系
                        msg.pose.orientation.w = 0.0
                        # print(len(result))
                        if len(result) >= 1:
                            msg.pose.position.x = float(result[0])  # 第1个元素为 x 坐标
                            msg.pose.position.y = float(result[1]) if len(result) > 1 else 0.0  # 第2个元素为 y 坐标，若数据不足则为 0
                            msg.pose.position.z = float(result[2]) if len(result) > 2 else 0.0  # 第3个元素为 z 坐标，若数据不足则为 0
                        # 如果数据长度为 4 到 7，使用接下来的四个元素设置姿态（四元数）
                        if len(result) >= 4:
                            # 四元数部分
                            msg.pose.orientation.x = float(result[3]) if len(result) > 3 else 0.0
                            msg.pose.orientation.y = float(result[4]) if len(result) > 4 else 0.0
                            msg.pose.orientation.z = float(result[5]) if len(result) > 5 else 0.0
                            msg.pose.orientation.w = float(result[6]) if len(result) > 6 else 0.0
                        # self.publisher_cmd.publish(msg)
                        print(msg)
                    else:
                        print(f"ID {self.uav_id} not found.")
                        msg = CommandMsg()
                        msg.stamp = self.get_clock().now().to_msg()
                        msg.frame_id = "stop"  # 可以根据需要调整坐标系
                        # self.publisher_cmd.publish(msg)
                        print(msg)
                
                elif head == 6:
                    #print(data)
                    # print(len(data))
                    # 解包数据，'d' 表示 double（8字节）
                    unpacked_data = struct.unpack("16sddd", data[16:])
                    #print(unpacked_data)
                    frame_id = unpacked_data[0].rstrip(b'\x00').decode()
                    pos_x = unpacked_data[1]
                    pos_y = unpacked_data[2]
                    pos_z = unpacked_data[3]
                    cmd = CommandMsg()
                    cmd.frame_id = frame_id
                    cmd.pose.position.x = pos_x
                    cmd.pose.position.y = pos_y
                    cmd.pose.position.z = pos_z
                    self.publisher_cmd.publish(cmd)
                    print(cmd)

                elif head == 0:
                    unpacked_data = struct.unpack('iiiddddd32s32s', data[16:])

                    recType = unpacked_data[0]
                    para1 = unpacked_data[1]
                    para2 = unpacked_data[2]
                    para3 = unpacked_data[3]
                    para4 = unpacked_data[4]
                    para5 = unpacked_data[5]
                    para6 = unpacked_data[6]
                    para7 = unpacked_data[7]
                    para8 = unpacked_data[8].decode('utf-8')
                    uavName = unpacked_data[9].decode('utf-8')

                    # 输出接收到的内容
                    print(f"Received data from {address}:\n recType={recType}, para1={para1}, para2={para2}, "
                        f"para3={para3}, para4={para4}, para5={para5}, para6={para6}, "
                        f"para7={para7}, para8={para8}, uavName={uavName}")
                    
                    msg_goal = PoseStamped()
                    
                    msg_goal.header.stamp = self.get_clock().now().to_msg()  # 设置时间戳
                    msg_goal.header.frame_id = str(para2)  # 队形

                    # 设置位置
                    msg_goal.pose.position.x = para3
                    msg_goal.pose.position.y = para4
                    msg_goal.pose.position.z = para5
                    self.publisher_cmd.publish(msg_goal)
                # 发布消息                

            except socket.timeout:
                pass
            except Exception as e:
                self.get_logger().error(f"{e}\ndata: {data.hex()}")
            




def main(args=None):
    rclpy.init(args=args)
    node = GoalNode()

    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.sock.close()
        node.destroy_node()   
        rclpy.shutdown() 
    
if __name__ == "__main__":
    main()    
